import cv2
import numpy as np
import math

class LineDetector:
    def __init__(self):
        """初始化参数"""
        self.min_area = 2000         # 降低最小轮廓面积阈值,以检测更小的线条
        self.max_area = 300000        # 增大最大轮廓面积阈值,以适应更大的线条
        self.roi_height_start = 0.35 # ROI区域开始
        self.roi_height_end = 0.65  # ROI区域终止
        self.line_mode = "double"     # 巡线模式:"single"为单线,"double"为双线
        self.target_width = 640       # 目标图像宽度
        self.target_height = 480      # 目标图像高度

    def detect_lines(self, image):
        """检测图像中的线条"""
        # 统一调整图像大小
        image = cv2.resize(image, (self.target_width, self.target_height))
        
        # 设置ROI区域
        height = image.shape[0]
        width = image.shape[1]
        roi_start = int(width * self.roi_height_start)
        roi_end = int(width * self.roi_height_end)
        roi = image[:, roi_start:roi_end]
        
        # 转换为灰度图
        gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
        
        # 二值化处理
        threshold = 80
        _, binary = cv2.threshold(gray, threshold, 255, cv2.THRESH_BINARY)
        
        # 反转图像使线条为白色
        binary = cv2.bitwise_not(binary)
        
        # 形态学操作增强线条
        kernel = np.ones((5,5), np.uint8)
        binary = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel, iterations=2)
        binary = cv2.morphologyEx(binary, cv2.MORPH_OPEN, kernel, iterations=1)

        # 查找轮廓
        contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        
        # 过滤轮廓
        valid_lines = []
        for contour in contours:
            area = cv2.contourArea(contour)
            if self.min_area < area < self.max_area:
                # 调整坐标以适应原图
                valid_lines = []
                # 只需对 x 方向加上 roi_start
                contour[:, 0, 0] = contour[:, 0, 0] + roi_start
                valid_lines.append(contour)

        return valid_lines if valid_lines else None

    def process_frame(self, frame):
        """处理单帧图像"""
        # 统一调整图像大小
        frame = cv2.resize(frame, (self.target_width, self.target_height))
        image_height, image_width = frame.shape[:2]
        # 默认中心在画面中线
        center = image_height // 2
        target_center = image_height // 2
        cnt = 0
        left_top = 0
        right_top = 0

        # 检测轮廓
        lines = self.detect_lines(frame)  # 使用detect_lines方法检测线条
        print(f"检测到 {len(lines)} 条线")

        # 丢线计数
        if not lines:
            pass
          
        else:
            # 绘制所有轮廓及其质心（调试用）
            for contour in lines:
                cv2.drawContours(frame, [contour], -1, (0,255,0), 2)
               
                M = cv2.moments(contour)
                if M['m00'] != 0:
                    cx = int(M['m10']/M['m00'])
                    cy = int(M['m01']/M['m00'])
                    cv2.circle(frame, (cx, cy), 4, (0,255,0), -1)

            # 单线模式：
            if len(lines) == 1:
                contour = lines[0]
                # 计算整个轮廓的质心位置
                M = cv2.moments(contour)
                if M['m00'] != 0:
                    contour_cx = int(M['m10']/M['m00'])
                    contour_cy = int(M['m01']/M['m00'])
                else:
                    contour_cx = image_width  // 2
                    contour_cy = image_height // 2  # 使用整数除法确保结果为整数，避免浮点数坐标

                #检测是否进入弯道，通过angle_deg

                contour = lines[0].reshape(-1, 2)
                
                # 动态容差计算（基于图像高度）
                y_tolerance = max(10, int(image_height * 0.1))  # 至少5像素，不超过图像高度的5%
                
                # 相位1：基础筛选
                min_y = np.min(contour[:, 1])
                top_points = contour[contour[:, 1] <= min_y + y_tolerance]
                
                # 相位2：有效性校验
                if len(top_points) >= 2:
                    # 计算有效宽度
                    effective_width = np.ptp(top_points[:, 0])
                    
                    # 当有效宽度不足时启用增强模式
                    if effective_width < 50:  # 根据实际场景调整阈值
                        # 增强模式：凸包极值点检测
                        hull = cv2.convexHull(contour)
                        hull_points = hull.reshape(-1, 2)
                        
                        # 寻找y最小的凸包点集合
                        hull_min_y = np.min(hull_points[:, 1])
                        hull_top = hull_points[hull_points[:, 1] <= hull_min_y + y_tolerance]
                        
                        # 二次筛选有效点
                        if len(hull_top) >= 2:
                            left_top = hull_top[np.argmin(hull_top[:, 0])]
                            right_top = hull_top[np.argmax(hull_top[:, 0])]
                        else:
                            # 保底逻辑：直接取轮廓极值
                            left_top = contour[np.argmin(contour[:, 0])]
                            right_top = contour[np.argmax(contour[:, 0])]
                    else:
                        # 正常情况取基础筛选结果
                        left_top = top_points[np.argmin(top_points[:, 0])]
                        right_top = top_points[np.argmax(top_points[:, 0])]
                else:
                    # 异常处理：使用质心投影
                    M = cv2.moments(contour)
                    cx = int(M['m10']/M['m00']) if M['m00'] else image_width//2
                    left_top = (max(0, cx-20), min_y)  # 左侧偏移
                    right_top = (min(image_width, cx+20), min_y)  # 右侧偏移

                # 角度计算优化（防止零除）
                dx = max(right_top[0] - left_top[0], 1e-5)  # 确保非零
                dy = right_top[1] - left_top[1]
                angle_deg = math.degrees(math.atan2(dy, dx))
                
                # 可视化验证
                cv2.polylines(frame, [contour], True, (0,255,0), 2)  # 绘制原始轮廓
                cv2.circle(frame, tuple(left_top), 7, (255,0,0), -1)
                cv2.circle(frame, tuple(right_top), 7, (0,0,255), -1)
                cv2.line(frame, tuple(left_top), tuple(right_top), (0,255,255), 2)
                # cv2.putText(frame, f"angle: {angle_deg:.2f}°", (10,100),)
                print(f"左上点：{tuple(left_top)}，右上点：{tuple(right_top)}，倾斜角：{angle_deg:.2f}°")

                
                center = contour_cy #实际偏移值

        # 画出目标中心线
        ty = int(target_center)
        # 在整张图上以 ty 为“高度”画水平中线（分成若干小段，更醒目）
        for x in range(0, image_width, 10):
            x_end = min(x + 8, image_width - 1)
            cv2.line(
                frame,
                (x, ty),          # 起点：(x, ty)
                (x_end, ty),      # 终点：(x_end, ty)
                (0, 0, 255),      # 红色
                2                 # 线宽
            )
        # 计算偏差、平滑、映射为角度
        deviation = center - target_center
        # 显示结果
        cv2.circle(frame, (int(image_width // 2),int(center)), 5, (0,0,255), -1)  
        cv2.putText(frame, f"deviation: {deviation:.2f}", (10,30),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,255), 1)

        return frame, deviation
    
if __name__ == "__main__":

    image_path = r"/home/yx/python_project1/234.jpg"   # ← 改成你的图片文件路径
    frame = cv2.imread(image_path)
    if frame is None:
        print(f"无法读取图片：{image_path}")
        exit(1)

    # 2. 初始化检测器并处理这一帧
    detector_line = LineDetector()
    # 假设你的类里有个 process(frame) 或 detect(frame) 方法做整套检测+绘制
    frame_result,result = detector_line.process_frame(frame)

    # # 3. 显示结果
    # cv2.imshow("Detection Result",frame_result)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()

    #4. 可选：将结果保存到硬盘
    output_path = r"/home/yx/python_project1/result.jpg"  # ← 如果不需要可删掉下面两行
    cv2.imwrite(output_path, frame_result)
    print(f"检测结果已保存到：{output_path}")
    
